function ret = inverse_kin(a)
    
    %%%%%%%%%%%%%%%%%%%%% Numeric inverse kinematics
    %%%%%q0 as computed from the human parameters
    load('x_ic.mat') 
    ic = x_opt;
% % % %     ic(3) = -ic(3);
% % % %     q0 = ic(1:4,:);
    q0 = [   -0.5295
              0.2821
              0.6505%0.6505
              0.0059];
    %%%Numerically solving for the initial condition
    
    options = optimset('Display','off');
    qzero = fsolve(@(q)inverse_kin_fun(q,a),q0,options);
    % %%%%If you update the inital guess on each iteration, the optimization does
    % %%%%not converge
    % q0 = qzero
    ret = qzero;
    

end
function F = inverse_kin_fun(q,a)

phipcond = phip_sca(R(q));
y2plus = ya2_vec(R(q), phipcond, a)-yd2_vec(R(q), phipcond,  a);

F = [y2plus;
     h_sca(q)];

% F = [y_plus(q,a);
%      h_sca(q)];

end

function ret = R(q)

dimq = length(q);
x = [q; zeros(dimq,1)];
xplus = resetFunc(x);

ret = xplus(1:dimq);    
        
end   